/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: BearingClient.cpp,v 1.12 2010-01-07 14:09:06 ray Exp $
 *
 * \file
 * \brief Implementation for Bearing interface client library.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include "BearingClient.hpp"
#include "BearingData.hpp"

//*****************************************************************************
using namespace MaCI::Bearing;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CBearingClient::CBearingClient(gimi::GIMI *aGIMIPtr, 
                           const int aInterfaceMinor)
  : CSync(1,1), // Max One Mutex, Max one Condition
    CThread(1), // Max one Thread.
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_BEARING, aInterfaceMinor),
    iBearingDataSequence(0),
    iBearingCommandSequence(0),
    iLastBearingData(),
    iBearingClientCallback(NULL)
{
  dPrint(4,"CBearingClient %p constructed", this);
}
//*****************************************************************************

CBearingClient::~CBearingClient()
{
  DoClose();
  dPrint(4,"CBearingClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CBearingClient::DoOpen(void)
{

  EMaCIError result = KMaCIError;
  
  // Checks.
  if(!IsActive()) {

    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if (result == KMaCIOK) {
      // Start the Thread
      EnableSynchronousMessageReply();
      SetActive(true);
      CThread::RunThread();
      dPrint(4,"CBearingClient::DoOpen() was succesfull.");
    }

  } else {
    dPrint(4,"BearingClient already Active, not Opening again.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CBearingClient::DoClose(void)
{
  EMaCIError result = KMaCIError;
  
  if(IsActive()) {
    SetActive(false);

    // Wait for thread to complete.
    CThread::SetRequestTermination();
    /* CThread::CancelThread(); //Removed by script */
    CThread::WaitThread();
    result = UnsubscribeToInterface(TimeoutValue(2000));
  
    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed BearingClient");    
    }else{
      dPrint(4,"Unsubscribing failed!");
    }
   
    
  } else {
    dPrint(4, "BearingClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************
bool CBearingClient::GetParamBySynchronousCall(unsigned int &aNumber,
                                                  unsigned int parameter, 
                                                  unsigned int aTimeout_ms)
{
  bool result = false;
  CBearingData rd;
  CBearingData dd;
  uint32_t data;

  data=(uint32_t)parameter<<24;

  dd.CreateInternalBinBag();

  ++iBearingCommandSequence;
      
  // Set timestamp to current (default constructor of Common::TTimestamp)
  dd.SetTimestamp(Common::TTimestamp());
  
  // Add a bearing command
  dd.SetCommand(TCommand(KCommandGetParam));
  dd.SetBlock(TBlock(data));
  // Send! ( Should probably check the Send() - but timeouted receive
  // gives the same )
  SendCommand(dd, iBearingCommandSequence);

  // Now, get reply! (Give it 1000ms time to answer)
  GetCommandReply(rd, iBearingCommandSequence, aTimeout_ms);


  // Extract data.
  const TBlock *de = rd.GetBlock();
  if (de) {
    aNumber = (unsigned int)(de->data & 0xFFFF);     //Store parameter value
    dPrint(2,"got param %x with value %x",de->data>>24,aNumber);
    if ((de->data>>24)==parameter) {  //Check for reception of correct parameter
      result = true;
    }
  }

  return result;
}

//*****************************************************************************
bool CBearingClient::GetTagsBySynchronousCall(unsigned int &aNumber,
                                                  unsigned int numTags, 
                                                  unsigned int aTimeout_ms)
{
  bool result = false;
  CBearingData rd;
  CBearingData dd;
  uint32_t data;
  char tags;

  data=(uint32_t)numTags;

  dd.CreateInternalBinBag();

  ++iBearingCommandSequence;
      
  // Set timestamp to current (default constructor of Common::TTimestamp)
  dd.SetTimestamp(Common::TTimestamp());
  
  // Add a bearing command
  dd.SetCommand(TCommand(KCommandGetTags));
  dd.SetBlock(TBlock(data));
  // Send! ( Should probably check the Send() - but timeouted receive
  // gives the same )
  SendCommand(dd, iBearingCommandSequence);

  // Now, get reply! (Give it 1000ms time to answer)
  GetCommandReply(rd, iBearingCommandSequence, aTimeout_ms);


  // Extract data.
  const TBlock *de = rd.GetBlock();
  if (de) {
    tags=(char)(de->data && 0xFF);
    if (tags>0) {
      aNumber = (unsigned int)(de->data>>8);     //Store parameter value
      //dPrint(2,"got %d tags. First ID %x",tags,aNumber);
      result = true;
    }
    else {
      // dPrint(2,"NOTHING");
    }
  }

  return result;
}
//*****************************************************************************
bool CBearingClient::GetEPCBySynchronousCall(unsigned char aEPC[12], 
                                             unsigned int aTimeout_ms)
{
  bool result = false;
  CBearingData rd;
  CBearingData dd;
  uint32_t data;
  //char tags;
  int k;
  unsigned char *tptr;

  data=2;

  dd.CreateInternalBinBag();

  ++iBearingCommandSequence;
      
  // Set timestamp to current (default constructor of Common::TTimestamp)
  dd.SetTimestamp(Common::TTimestamp());
  
  // Add a bearing command
  dd.SetCommand(TCommand(KCommandRead));
  dd.SetBlock(TBlock(data));
  // Send! ( Should probably check the Send() - but timeouted receive
  // gives the same )
  SendCommand(dd, iBearingCommandSequence);

  // Now, get reply! (Give it 1000ms time to answer)
  GetCommandReply(rd, iBearingCommandSequence, aTimeout_ms);

  // Extract data.
  const TTriBlock *de = rd.GetTriBlock();
  if (de) {
    //tags=(char)(de->dataC && 0xFF);
    //if (tags>0) {
      //aNumber = (unsigned int)(de->data>>8);     //Store parameter value
      /*      
data=de->dataC;
      memcpy(&aEPC[8],&data,4);
      data=de->dataB;
      memcpy(&aEPC[4],&data,4);
      data=de->dataA;
      memcpy(&aEPC[0],&data,4);*/
      tptr=(unsigned char*)&de->dataC;
      for (k=0; k<4; k++) aEPC[11-k]=tptr[k];
      tptr=(unsigned char*)&de->dataB;
      for (k=0; k<4; k++) aEPC[7-k]=tptr[k];
      tptr=(unsigned char*)&de->dataA;
      for (k=0; k<4; k++) aEPC[3-k]=tptr[k];   
      //dPrint(2,"got %d tags. First ID %x",tags,aNumber);
      result = true;
    //}
    //else {
      // dPrint(2,"NOTHING");
      //}
  }

  return result;
}

//*****************************************************************************

bool CBearingClient::GetBearingEvent(CBearingData &aData,
                                 unsigned int *aSeq,
                                 const unsigned int aTimeout_ms)
{  
  bool result = false;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return false;

  Lock();
  
  if (aSeq == NULL) {
    // No sequence number defined. Just do direct copy of local data - no sync
    aData = iLastBearingData;
    result = true;
    
  } else {
    
    if (iBearingDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();

      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left, KEventBearingData);
	time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iBearingDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iBearingDataSequence > *aSeq) {
	// Yes :)
	aData = iLastBearingData;
	*aSeq = iBearingDataSequence;
	result = true;
        //dPrint(1,"Got data. *aSeq = %d, iBearingDataSequence = %d", *aSeq, iBearingDataSequence);
	
      } else {
	// No :(
	dPrint(10, "Failed to get BearingData! (Timeout %dms)",
	       aTimeout_ms);
      }
      
    } else {
      // The available data is newer or matching.
      aData = iLastBearingData;
      *aSeq = iBearingDataSequence;
      //      dPrint(1,"Got data. *aSeq = %d, iBearingDataSequence = %d", *aSeq, iBearingDataSequence);
      result = true;
    }
  }
  
  Unlock();
  
  return result;
}
//*****************************************************************************
//*****************************************************************************
/**
 * This function is an _EXAMPLE_ of a directly receiving
 * function. When this function model is used, the Client class
 * doesn't have to implement a separate thread. (ProcessMessage and
 * ThreadFunction are not used). However, this means that no special
 * processing can be done by the interface hence this model is only
 * suitable for simple interfaces. This can ofcourse be later changed
 * without changing the public interface, so it's not a
 * architecturally significant decision. Use of this model is also
 * encouraged if possible, as it saves one extra thread thus saving a
 * (minimal) fraction of system resources.
 *
 * As this BearingClient uses the more complex processing model as
 * example, this code block is disabled with precompiler directive,
 * but left here as on fully working example of receiving MaCI
 * dataelements directly from GIMI.
 */
#if 0
bool CBearingClient::GetBearingEvent(CBearingData &aData,
                                 const unsigned int aTimeout_ms);
{
  EMaCIError result;
  gimi::GIMIMessage gmsg;
  
  if (IsActive()) {
    
    // Call the ReceiveMessage wrapper with 2s timeout
    result = ReceiveInterfaceMessage(gmsg, aTimeout_ms);
    
    // If result is OK, process the msg.
    if (result == KMaCIOK) {
      bool r;
      gim::binbag::CBinBag bb;
      CBearingData d;
      
      // Decode BinBag.
      r = bb.Decode(gmsg.getData(), gmsg.getDatalength());
      if (r) {
        // BinBag OK. Decode BearingData.
        r = d.DecodeFrom(&bb);
        if (r && d.GetBearingEvent()) {
          // BearingData ok. Copy to caller.
          aData.Copy(d);
          
        } else {
          result = KMaCIInvalidFormat;
          
        }
        
      } else {
        result = KMaCIInvalidFormat;
        
      }
    }
  } else {
    result = KMaCINotActive;

  }
  
  return (result == KMaCIOK);
}
#endif
//*****************************************************************************
//*****************************************************************************

bool CBearingClient::GetCommandReply(CBearingData &aData, const int aMessageID, 
                                   const int aTimeout_ms)
{ 
  EMaCIError result;
  gimi::GIMIMessage gmsg;
  
  // Call the ReceiveMessage wrapper with 2s timeout
  result = ReceiveSynchronousMessageReply(gmsg, aMessageID, aTimeout_ms);
  
  // If result is OK, process the msg.
  if (result == KMaCIOK) {
    bool r;
    gim::binbag::CBinBag bb;
    CBearingData d;

    // Decode BinBag.
    r = bb.Decode(gmsg.getData(), gmsg.getDatalength());
    if (r) {
      // BinBag OK. Decode BearingData.
      r = d.DecodeFrom(&bb);
      if (r) {
        // BearingData ok. Copy to caller.
        aData.Copy(d);

      } else {
        result = KMaCIInvalidFormat;

      }

    } else {
      result = KMaCIInvalidFormat;
      
    }
  }
  
  return (result == KMaCIOK);
}

//*****************************************************************************

bool CBearingClient::SendCommand(const CBearingData &aBearingData, const int aMessageID)
{
  EMaCIError result = KMaCIError;
  
  if (IsActive()) {
    if (aBearingData.GetCommand()) {
      // Foo.
      ++iBearingCommandSequence; // Only this func. thread is updating this, so no locks required.
      
      // Send.
      SendInterfaceMessage(aBearingData.GetBinBagContainerPtr()->GetBuffer(),
                           aBearingData.GetBinBagContainerPtr()->GetBufferSize(),
                           iBearingCommandSequence);
      
      dPrint(4,"SendInterfaceMessage called: %d:%d, seq: %d (%d bytes of payload sent)",
             iInterfaceMajor, iInterfaceMinor, 
             iBearingCommandSequence-1,
             aBearingData.GetBinBagContainerPtr()->GetBufferSize());
      // Result
      result = KMaCIOK;
      
    } else {
      dPrint(2,"No 'TCommand' element in provided CBearingData object. Will not send!");
      result = KMaCIInvalidFormat;

    }
   
  } else {
    dPrint(2,"Unable to send BearingData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;

  }


  return (result == KMaCIOK);
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CBearingClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CBearingData id;
  bool r;

  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    r = id.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as Bearing, now handle contents
      Lock();
      if (id.GetBearingEvent()) {
        iLastBearingData = id;
        ++iBearingDataSequence;
        Notify(KEventBearingData);
        dPrint(10,"Got BearingData, Sequence now %u", iBearingDataSequence);
      }
      Unlock();
      
      // Call the handler if set.
      if (iBearingClientCallback) {
        dPrint(8,"Got Callback for BearingDataEvent - executing!");
        iBearingClientCallback->OnBearingDataEvent(id,
                                               iBearingDataSequence,
                                               *this);
        dPrint(8,"Callback for BearingDataEvent - execution complete!");
      } 

    } else {
      dPrint(4,"WARNING: Received Bearing interface message, but failed to decode as BearingData!");
      
    }
  }
  
  return result;
}

//*****************************************************************************

int CBearingClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  const unsigned int hard_error_count_limit = 10;
  unsigned int hard_error_count = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(8, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000);
    if (e == KMaCIOK) {
      ProcessMessage(gmsg);

      // Reset error counter.
      if (hard_error_count) hard_error_count = 0;

    } else if (e != KMaCITimeout) {
      // Increment hard error count
      ++hard_error_count;
      
      // Check whether the hard error count limit is still under limit. 
      if (hard_error_count < hard_error_count_limit) {
        // Other error, try to recover by sleeping one second.
        dPrint(10,"ProcessMessage returned error %u, trying to recover by sleeping 1000ms (%u attempts left)",
               e, hard_error_count_limit-hard_error_count);
        ownSleep_ms(1000);

      } else {
        dPrint(3,"Error Recovery failed, BearingClient ThreadFunction terminating...");

        // Break the while().
        break;
      }
      
    }
  }
  
  dPrint(8, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
